#face_tracking_PID.py
#-*- coding: UTF-8 -*-	
# 调用必需库
from multiprocessing import Manager
from multiprocessing import Process
from objcenter import ObjCenter
from pid import PID
from gpiozero import AngularServo as Servo
import argparse
import signal
import time
import sys
import cv2
# from picamera2 import Picamera2


# 定义舵机
pan=Servo(pin=19, min_angle=-90, max_angle=90)
tilt=Servo(pin=16, min_angle=-90, max_angle=90)

#定义图像尺寸
dispW=1280
dispH=720

      
# 键盘终止函数
def signal_handler(sig, frame):
    # 输出状态信息
    print("[INFO] You pressed `ctrl + c`! Exiting...")

    # 关闭舵机
    pan.close()
    tilt.close()

    # 退出
    sys.exit()

def obj_center(objX, objY, centerX, centerY):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    # 启动视频流并缓冲
    print("[INFO] waiting for camera to warm up...")
    cv2.startWindowThread()
    # picam2 = Picamera2()
	# preview_config = picam2.create_preview_configuration(main={"size": (dispW, dispH),"format":"RGB888"})
    # picam2.configure(preview_config)
    # picam2.start()
    
    # 1. 打开摄像头（0 表示默认摄像头，外接摄像头可能需改 1/2 等）
    cap = cv2.VideoCapture(0)  

    # 2. 检查摄像头是否成功打开
    if not cap.isOpened():
        print("无法打开摄像头")
        exit()
    
    time.sleep(2.0)

    # 初始化人脸中心探测器
    obj = ObjCenter("./haarcascade_frontalface_default.xml")
    # 进入循环
    while True:

        # # 从视频流抓取图像并旋转
        # frame= picam2.capture_array()
        
        # 3. 逐帧读取
        ret, frame = cap.read()  
        # ret: 是否读取成功；frame: 视频帧数据（numpy 数组）
        
        if not ret:
            print("无法获取视频帧，结束")
            break
    
        frame = cv2.flip(frame, 1)

        # 找到图像中心
        (H, W) = frame.shape[:2]
        centerX.value = W // 2
        centerY.value = H // 2

        #draw a point in the center of frame
        cv2.circle(frame, (centerX.value, centerY.value), 5, (0, 0, 255), -1)

        # 找到人脸中心
        objectLoc = obj.update(frame, (centerX.value, centerY.value))
        ((objX.value, objY.value), rect) = objectLoc
        print("objx.value", objX.value)
        print("objy.value", objY.value)

        # 绘制人脸外界矩形
        if rect is not None:
            (x, y, w, h) = rect
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
            fX = int(x + (w / 2.0))
            fY = int(y + (h / 2.0))
            cv2.circle(frame, (fX, fY), 5, (0, 0, 255), -1)
            # 在人脸中心和视窗中心画一条连线
            cv2.line(frame, (centerX.value, centerY.value),
                     (fX, fY), (0, 255, 0), 2)
            # 显示图像
        cv2.imshow("Pan-Tilt Face Tracking", frame)
        cv2.waitKey(1)

def pid_process(output, p, i, d, objCoord, centerCoord):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)
    
     # 创建一个PID类的对象并初始化
    p = PID(p.value, i.value, d.value)
    p.initialize()

    # 进入循环
    while True:
        # 计算误差
        error = centerCoord.value - objCoord.value
        # 更新输出值,当error小于50时，误差设为0，以避免云台不停运行。
        if abs(error) < 50:
            error = 0
        output.value = p.update(error)


def set_servos(panAngle, tiltAngle):
    # ctrl+c退出进程
    signal.signal(signal.SIGINT, signal_handler)

    #进入循环
    while True:
        # 偏角变号
        yaw = -1 * panAngle.value
        pitch = -1 * tiltAngle.value
        print(f"yaw {yaw},pitch {pitch}")
        # 设置舵机角度。
        pan.angle = yaw
        tilt.angle = pitch
            

# 启动主程序
if __name__ == "__main__":
    # 启动多进程变量管理
    with Manager() as manager: #相当于manager=Manager(),with as 语句操作上下文管理器（context manager），它能够帮助我们自动分配并且释放资源。
        # 舵机角度置零
        pan.angle = 0
        tilt.angle = 0
        
        # 为图像中心坐标赋初值
        centerX = manager.Value("i", 0) #"i"即为整型integer
        centerY = manager.Value("i", 0)

        # 为人脸中心坐标赋初值
        objX = manager.Value("i", 0)
        objY = manager.Value("i", 0)

        # panAngle和tiltAngle分别是两个舵机的PID控制输出量	    
        panAngle = manager.Value("i", 0)
        tiltAngle = manager.Value("i", 0)

       # 设置一级舵机的PID参数
        panP = manager.Value("f", 0.015)  # "f"即为浮点型float
        panI = manager.Value("f", 0.01)
        panD = manager.Value("f", 0.0008)

        # 设置二级舵机的PID参数
        tiltP = manager.Value("f", 0.025)
        tiltI = manager.Value("f", 0.01)
        tiltD = manager.Value("f", 0.008)

        # 创建4个独立进程
        # 1. objectCenter  - 探测人脸
        # 2. panning       - 对一级舵机进行PID控制，控制偏航角
        # 3. tilting       - 对二级舵机进行PID控制，控制俯仰角
        # 4. setServos     - 根据PID控制的输出驱动舵机
                          
        processObjectCenter = Process(target=obj_center,args=(objX, objY, centerX, centerY))
        processPanning = Process(target=pid_process,args=(panAngle, panP, panI, panD, objX, centerX))
        processTilting = Process(target=pid_process,args=(tiltAngle, tiltP, tiltI, tiltD, objY, centerY))
        processSetServos = Process(target=set_servos, args=(panAngle, tiltAngle))

        # 开启4个进程
        processObjectCenter.start()
        processPanning.start()
        processTilting.start()
        processSetServos.start()

        # 添加4个进程
        processObjectCenter.join()
        processPanning.join()
        processTilting.join()
        processSetServos.join()

